// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KukaLBRControlAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.Runtime
{
    using System;
    using System.Diagnostics;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Hardware;
    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Main arm agent. Publishes status messages and handles control messages for arm
    /// </summary>
    [DataContract]
    public class KukaLBRControlAgent : 
                            ConsumerProducerAgent<TimerMessage, ManipulatorStatusMessage>,
                            IControlReceiver<LinearCartesianMotionControlMessage>,
                            IControlReceiver<CartesianGoalPositionControlMessage>,
                            IControlReceiver<LinearCartesianTouchControlMessage>,
                            IControlReceiver<LinearCartesianPushControlMessage>,
                            IControlReceiver<JointGoalPositionControlMessage>,
                            ISubscriptionReceiver<ArmStopMessage>,
                            IControlReceiver<MoveToHomePositionMessage>
    {
        /// <summary>
        /// Maximum Cartesian linear velocity
        /// </summary>
        [DataMember(Name = "MaxLinearVelocity")]
        private double maxLinearVelocity;

        /// <summary>
        /// Maximum Cartesian angular velocity
        /// </summary>
        [DataMember(Name = "MaxAngularVelocity")]
        private double maxAngularVelocity;

        /// <summary>
        /// Maximum joint move velocity in radians per second
        /// </summary>
        [DataMember(Name = "MaxJointVelocity")]
        private double maxJointVelocityRadPerSecond;

        /// <summary>
        /// Indicates whether or not to use KRL to generate motion commands.
        /// </summary>
        [DataMember(Name = "UseKrlMotionCommands")]
        private bool useKrlMotionCommands;

        /// <summary>
        /// region accessibility checker to ensure the goal is safe to reach
        /// </summary>
        [DataMember(Name = "AccessibilityChecker")]
        private RegionAccessibilityChecker accessibilityChecker;

        /// <summary>
        /// Minimum duration of a joint motion (specified in terms of FRI control cycles).
        /// </summary>
        [DataMember(Name = "JointMotionDurationInCycles")]
        private double jointMotionDurationInCycles;

        /// <summary>
        /// Initializes a new instance of the <see cref="KukaLBRControlAgent" /> class
        /// </summary>
        /// <param name="name">The name</param>
        /// <param name="producer">heartbeat generator</param>
        /// <param name="maxLinearVelocityInMetersPerSec"> Maximum linear velocity of tool control point in m/s. </param>
        /// <param name="maxAngularVelocityInRadPerSec"> Maximum angular velocity of tool control point in rad/s. </param>
        /// <param name="maxJointVelocityRadPerSec">Maximum joint velocity for arm movement in radians per sec</param>
        /// <param name="useKrlMotionCommands"> Indicates whether or not to use KRL to generate motion commands.</param>
        /// <param name="jointMotionDurationInCycles"> 
        /// Duration in FRI control cycles that motion will span. For example, if FRI cycle time is 10ms and duration is 3 cycles, 
        /// then all joint motions will attempt to be completed in exactly 30ms subject to limits on joint velocity. If joint position was not
        /// achievable within 30ms, motion will continue (past 30ms) until target position is reached.
        /// </param>
        /// <param name="accessibilityChecker">Region accessibility checker to ensure the goal is safe</param>
        public KukaLBRControlAgent(
            string name,
            IProducer<TimerMessage> producer,
            double maxLinearVelocityInMetersPerSec,
            double maxAngularVelocityInRadPerSec,
            double maxJointVelocityRadPerSec,
            bool useKrlMotionCommands,
            double jointMotionDurationInCycles,
            RegionAccessibilityChecker accessibilityChecker = null)
            : base(name, producer.Name)
        {
            this.maxLinearVelocity = maxLinearVelocityInMetersPerSec;
            this.maxAngularVelocity = maxAngularVelocityInRadPerSec;
            this.maxJointVelocityRadPerSecond = maxJointVelocityRadPerSec;
            this.useKrlMotionCommands = useKrlMotionCommands;
            this.accessibilityChecker = accessibilityChecker;
            this.jointMotionDurationInCycles = jointMotionDurationInCycles;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="KukaLBRControlAgent" /> class
        /// </summary>
        /// <param name="name">The name</param>
        /// <param name="producer">heartbeat generator</param>
        /// <param name="armControl">Arm controller to use</param>
        /// <param name="maxLinearVelocityInMetersPerSec"> Maximum linear velocity of tool control point in m/s. </param>
        /// <param name="maxAngularVelocityInRadPerSec"> Maximum angular velocity of tool control point in rad/s. </param>
        /// <param name="maxJointVelocityRadPerSec">Maximum joint velocity for arm movement in radians per sec</param>
        /// <param name="useKrlMotionCommands"> Indicates whether or not to use KRL to generate motion commands.</param>
        /// <param name="jointMotionDurationInCycles"> Duration in FRI control cycles that a single joint motion will span. </param>
        /// <param name="accessibilityChecker">Region accessibility checker to ensure the goal is safe</param>
        internal KukaLBRControlAgent(
            string name,
            IProducer<TimerMessage> producer,
            ArmControlKuka armControl,
            double maxLinearVelocityInMetersPerSec,
            double maxAngularVelocityInRadPerSec,
            double maxJointVelocityRadPerSec,
            bool useKrlMotionCommands,
            double jointMotionDurationInCycles,
            RegionAccessibilityChecker accessibilityChecker = null)
            : base(name, producer.Name)
        {
            this.ArmControl = armControl;
            this.maxLinearVelocity = maxLinearVelocityInMetersPerSec;
            this.maxAngularVelocity = maxAngularVelocityInRadPerSec;
            this.maxJointVelocityRadPerSecond = maxJointVelocityRadPerSec;
            this.useKrlMotionCommands = useKrlMotionCommands;
            this.accessibilityChecker = accessibilityChecker;
            this.jointMotionDurationInCycles = jointMotionDurationInCycles;
        }

        /// <summary>
        /// Gets or sets Arm control library
        /// </summary>
        internal ArmControlKuka ArmControl { get; set; }

        /// <summary>
        /// Initializes the agent
        /// </summary>
        /// <param name="locator">Agent locator to use</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
        }

        /// <summary>
        /// Called when the agent is about to start running
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();

            if (this.maxLinearVelocity <= 0.0)
            {
                throw new ArgumentException(string.Format("Maximum Cartesian linear velocity must be > 0 Fount  {0}", this.maxLinearVelocity));
            }

            if (this.maxAngularVelocity <= 0.0)
            {
                throw new ArgumentException(string.Format("Maximum Cartesian angular velocity must be > 0 Fount  {0}", this.maxAngularVelocity));
            }

            if (this.maxJointVelocityRadPerSecond <= 0.0)
            {
                throw new ArgumentException(string.Format("Maximum joint velocity must be > 0 Fount  {0}", this.maxJointVelocityRadPerSecond));
            }

            if (null == this.ArmControl)
            {
                this.ArmControl = new ArmControlKuka();
            }
        }

        /// <summary>
        /// Called when the agent is about to end
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
            this.ArmControl.Dispose();
        }

        /// <summary>
        /// Receive heartbeat message and publish status
        /// </summary>
        /// <param name="timerMessage">Message to process</param>
        public override void Receive(TimerMessage timerMessage)
        {
            ManipulatorStatus status = this.ArmControl.GetManipulatorStatus();

            if (status != null)
            {
                ManipulatorStatusMessage publishMsg = new ManipulatorStatusMessage(status, Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency);

                this.Publisher.Post(publishMsg);
            }
        }

        /// <summary>
        /// Receive and handle linear arm Cartesian move control message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void ReceiveControl(LinearCartesianMotionControlMessage message)
        {
            TraceOut.Info(TraceContexts.Manip, "Linear ArmMove: Pos {0:F3} {1:F3} {2:F3}", message.CartesianPosition.Position.X, message.CartesianPosition.Position.Y, message.CartesianPosition.Position.Z);

            TraceOut.Info(
                TraceContexts.Manip,
                " Orient {0:F3} {1:F3} {2:F3} {3:F3}",
                message.CartesianPosition.Orientation.W,
                message.CartesianPosition.Orientation.X,
                message.CartesianPosition.Orientation.Y,
                message.CartesianPosition.Orientation.Z);

            // check if the requested position is safe, if not, this motion request is ignored
            if (null != this.accessibilityChecker && !this.accessibilityChecker.IsSafe(message.CartesianPosition.Position))
            {
                string msg = string.Format(
                    "The given position [{0}, {1}, {2}] is not safe to reach according to the pre-set inaccessibible regions, abort!",
                    message.CartesianPosition.Position.X,
                    message.CartesianPosition.Position.Y,
                    message.CartesianPosition.Position.Z);
                System.Console.WriteLine(msg);
                return;
            }

            if (this.useKrlMotionCommands)
            {
                throw new NotImplementedException("KRL does not support linear motion, please use FRI.");
            }
            else
            {
                this.ArmControl.CartesianMoveTo(
                        message.CartesianPosition,
                        this.maxLinearVelocity,
                        this.maxAngularVelocity);
            }
        }

        /// <summary>
        /// Receive and handle arm Cartesian goal control message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void ReceiveControl(CartesianGoalPositionControlMessage message)
        {
            TraceOut.Info(TraceContexts.Manip, "ArmMove: Pos {0:F3} {1:F3} {2:F3}", message.CartesianPosition.Position.X, message.CartesianPosition.Position.Y, message.CartesianPosition.Position.Z);

            TraceOut.Info(
                TraceContexts.Manip,
                " Orient {0:F3} {1:F3} {2:F3} {3:F3}",
                message.CartesianPosition.Orientation.W,
                message.CartesianPosition.Orientation.X,
                message.CartesianPosition.Orientation.Y,
                message.CartesianPosition.Orientation.Z);

            // check if the requested position is safe, if not, this motion request is ignored
            if (null != this.accessibilityChecker && !this.accessibilityChecker.IsSafe(message.CartesianPosition.Position))
            {
                string msg = string.Format(
                    "The given position [{0}, {1}, {2}] is not safe to reach according to the pre-set inaccessibible regions, abort!",
                    message.CartesianPosition.Position.X,
                    message.CartesianPosition.Position.Y,
                    message.CartesianPosition.Position.Z);
                System.Console.WriteLine(msg);
                return;
            }

            if (this.useKrlMotionCommands)
            {
                this.ArmControl.KrlCartesianMoveTo(message.CartesianPosition);
            }
            else
            {
                this.ArmControl.CartesianMoveTo(
                    message.CartesianPosition,
                    this.maxLinearVelocity,
                    this.maxAngularVelocity);
            }
        }

        /// <summary>
        /// receive and handle arm cartesian touch control message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void ReceiveControl(LinearCartesianTouchControlMessage message)
        {
            // check if the requested position is safe, if not, this motion request is ignored
            if (null != this.accessibilityChecker && !this.accessibilityChecker.IsSafe(message.CartesianPosition.Position))
            {
                string msg = string.Format(
                    "The given position [{0}, {1}, {2}] is not safe to reach according to the pre-set inaccessibible regions, abort!",
                    message.CartesianPosition.Position.X,
                    message.CartesianPosition.Position.Y,
                    message.CartesianPosition.Position.Z);
                System.Console.WriteLine(msg);
                return;
            }

            this.ArmControl.CartesianTouch(
                    message.CartesianPosition,
                    message.MaxLinearCartesianSpeedInMetersPerSecond,
                    message.MaxRotationalCartesianSpeedInRadiansPerSecond,
                    message.ForceThresholdInNewtons);
        }

        /// <summary>
        /// receive and handle arm Cartesian push control message
        /// </summary>
        /// <param name="message">Message to process.</param>
        public void ReceiveControl(LinearCartesianPushControlMessage message)
        {
            // check if the requested position is safe, if not, this motion request is ignored
            if (null != this.accessibilityChecker && !this.accessibilityChecker.IsSafe(message.CartesianPosition.Position))
            {
                string msg = string.Format(
                    "The given position [{0}, {1}, {2}] is not safe to reach according to the pre-set inaccessibible regions, abort!",
                    message.CartesianPosition.Position.X,
                    message.CartesianPosition.Position.Y,
                    message.CartesianPosition.Position.Z);
                System.Console.WriteLine(msg);
                return;
            }

            this.ArmControl.CartesianPush(
                    message.CartesianPosition,
                    message.MaxLinearCartesianSpeedInMetersPerSecond,
                    message.MaxRotationalCartesianSpeedInRadiansPerSecond,
                    message.PushDirection,
                    message.PushForceThresholdInNewtons,
                    message.ResidualForceThresholdInNewtons);
        }

        /// <summary>
        /// receive and handle arm joint control message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void ReceiveControl(JointGoalPositionControlMessage message)
        {
            // TODO: need to check the goal specified by message is safe to reach
            this.ArmControl.JointPositionMove(
                    message.ArmJointPositions,
                    this.maxJointVelocityRadPerSecond,
                    this.jointMotionDurationInCycles);
        }

        /// <summary>
        /// receive and handle move to candle position message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void ReceiveControl(MoveToHomePositionMessage message)
        {
            this.ArmControl.MoveToCandlePosition(message.JointSpeedInRadiansPerSecond);
        }

        /// <summary>
        /// receive and handle arm stop message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void Receive(ArmStopMessage message)
        {
        }
    }
}
